// mdvctrl - A video-driven controller for MD-25 motor drivers
// Copyright (C) 2010  Nuno J. Silva
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, version 3 of the License.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// OpenCV headers
#include <cv.h>
#include <highgui.h>

#include <termios.h>

// error handling
#include <errno.h>
#include <string.h>
#include "fatal.h"

#include "serial.h"
#include "video.h"
#include "blobsteer.h"


// Define the period between frames, in nanoseconds (10^-6). (Inverse of the frame rate)
#define PERIOD 10000


#define UI_EXIT_KEY 'q'
#define UI_PAUSE_KEY ' '


// filenames for serial port device and log file.
#define LOG_FILENAME "/dev/null"
#define TTY_FILENAME "/dev/ttyUSB0"



int do_not_exit (void);

void save_stdin_settings (void );
void restore_stdin_settings (void );
void disable_cooked_stdin (void );


int main(int argc, char ** argv, char ** envp)
{
  // Tune stdin, but first backup current settings, so that we can
  // revert changes later.
  save_stdin_settings ();
  disable_cooked_stdin ();
  
  CvCapture * capture;
  CvVideoWriter * writer = NULL;
  opencv_init (NULL, NULL, &capture, &writer);

  // There's also open_md25_log (LOG_FILENAME), if you need to test
  // this without a real device.
  int robotfd = open_md25 (TTY_FILENAME);

  while (do_not_exit() != 0) {
    IplImage * frame = get_cooked_frame (capture, writer);

    // get_cooked_frame may return NULL if it is not possible to grab
    // a frame ATM.
    if (frame != NULL) {
      steer_blob(robotfd, get_best_blob (frame));
    }
  }

  print_stats ();

  // Let's be nice with the video subsystem...
  cvReleaseCapture (&capture);

  restore_stdin_settings ();

  return 0;
}


// Loop control: tells the loop when to exit. Also manages user
// requests to pause processing.
int do_not_exit (void) {
  fd_set streams;
  struct timeval delay;
  
  FD_ZERO (&streams);
  FD_SET (0, &streams);
  
  delay.tv_sec = 0;
  delay.tv_usec = PERIOD;
  
  if (select (1, &streams, NULL, NULL, &delay) != 0) {

    char buf;
    read (0, &buf, 1);

    switch (buf){
    case UI_EXIT_KEY:
      return 0;
      break;

    case UI_PAUSE_KEY:
      FD_ZERO (&streams);
      FD_SET (0, &streams);

      // Halt until some other key is read.
      select (1, &streams, NULL, NULL, NULL);
      read (0, &buf, 1);
      break;

    default:
      break;
    }   
  }
  return 1;
}


struct termios * input_settings = NULL;

void save_stdin_settings (void ) {
  input_settings = (struct termios *) malloc (sizeof (struct termios));

  if (input_settings == NULL)
    fatal ("virtual memory exhausted.");
  
  if (tcgetattr (STDIN_FILENO, input_settings) == -1) {
    int error = errno;
    fatal (strerror (errno));
  }
}


// Tune standard input so it suits our interaction needs (no buffering
// nor echoing)
void disable_cooked_stdin (void ) {
  struct termios * settings = NULL;
  settings = (struct termios *) malloc (sizeof (struct termios));
  
  if (settings == NULL)
    fatal ("virtual memory exhausted.");
  
  if (tcgetattr (STDIN_FILENO, settings) == -1) {
    int error = errno;
    fatal (strerror (errno));
  }
  
  // Disable canonical mode ...
  settings->c_lflag &= ~ICANON;
  // ... and echo
  settings->c_lflag &= ~ECHO;

  if (tcsetattr (STDIN_FILENO, TCSANOW, settings) == -1) {
    int error = errno;
    fatal(strerror(errno));
  } 
}

void restore_stdin_settings(void ) {
  if (tcsetattr (STDIN_FILENO, TCSANOW, input_settings) == -1) {
    int error = errno;
    fatal(strerror(errno));
  } 
}

